#include "brain.hpp"

void others_odom_cb(const nav_msgs::Odometry &msg)
{
  //get the drone id and flight state,formate: id/state
  std::vector<std::string> res;
  const char split = '/';
  std::string str = msg.child_frame_id;
  std::istringstream iss(str);	// 输入流
	std::string token;			// 接收缓冲区
	while (std::getline(iss, token, split))	// 以split为分隔符
	{
		res.push_back(token);
	}
  int id_temp = atoi(res[0].c_str());
  int state_temp = atoi(res[1].c_str());
  //根据飞机id进行归类
  vehicles.update_drone_state(id_temp, state_temp, msg);
  
}

void center_dispatch_callback(ros::TimerEvent e)
{
  std::vector<int> mission_vec(line_number, 0);
  //mission_vec:[3 0 0 0] 代表区块0上有3架飞机,这在起飞的时候显然是不合理的 ，飞机在arm状态下才能算在区块内
  //未解锁则state为-1
  for(int i = 0;i<line_number;i++)
  {
    for(int j = 0;j<vehicle_number;j++)
    {
      if(i == vehicles.selfstate_vec[j])
      {
        mission_vec[i]++;
      }
    }
  }
  //判断自身所在区块的下一区块是否被占用
  int self_qukuai = vehicles.selfstate_vec[droneid];
  int self_qukuai_next = self_qukuai+1;
  if(self_qukuai_next==line_number)
  {
    self_qukuai_next = 0;
  }
  //发布是否需要在执行完此区块任务后进行等待的命令 true为等待
  std_msgs::Bool ifneedwait;
  if(mission_vec[self_qukuai_next]==0)
  {
    ifneedwait.data = false;
  }
  else
  {
    ifneedwait.data = true;
  }
  needtowait_pub.publish(ifneedwait);
}

int main(int argc, char **argv)
{
  
  ros::init(argc, argv, "cluster_brain");
  ros::NodeHandle nh;

  //read parameters from launch file
  nh.param<int>("droneid",droneid,-1);
  nh.param<int>("vehicle_number",vehicle_number,10);
  nh.param<int>("line_number",line_number,10);

  vehicles.update_vehicle_number(vehicle_number);

  others_odom_sub = nh.subscribe("/others_odom", 10, others_odom_cb);

  needtowait_pub = nh.advertise<std_msgs::Bool>("/ifneedwait", 1);

  center_dispatch_timer = nh.createTimer(ros::Duration(0.1), center_dispatch_callback);

  ros::Rate rate(30);    
  while(ros::ok()){
    ros::spinOnce();
    rate.sleep();
  }
  return 0;
}


